#include <ros/ros.h>

#include "cg_interfaces/BridgeControl.h"
#include "cg_interfaces/BridgeSensor.h"
#include "cg_interfaces/ChassisStruct.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "local_loopback_test");
    ros::NodeHandle nl("~");
    std::string master_node;
    std::string slave_node;
    if (!nl.getParam("master_node", master_node))
    {
        ROS_ERROR("No master_node parameter found");
        ros::shutdown();
        return -1;
    }
    if (!nl.getParam("slave_node", slave_node))
    {
        ROS_ERROR("No slave_node parameter found");
        ros::shutdown();
        return -1;
    }

    ros::NodeHandle nh;
    ROS_INFO("Local loopback test node started");

    cg_interfaces::BridgeControl control_msg_rec;
    bool control_rec = false;
    cg_interfaces::BridgeSensor sensor_msg_rec;
    bool sensor_rec = false;
    auto control_pub = nh.advertise<cg_interfaces::BridgeControl>(master_node + "/control", 1);
    auto control_sub = nh.subscribe<cg_interfaces::BridgeControl>(
        slave_node + "/control", 1, [&](const cg_interfaces::BridgeControl::ConstPtr &msg) {
            control_msg_rec = *msg;
            control_rec = true;
            ROS_INFO("Received BridgeControl message");
        });
    auto sensor_pub = nh.advertise<cg_interfaces::BridgeSensor>(slave_node + "/sensor", 1);
    auto sensor_sub = nh.subscribe<cg_interfaces::BridgeSensor>(master_node + "/sensor", 1,
                                                                [&](const cg_interfaces::BridgeSensor::ConstPtr &msg) {
                                                                    sensor_msg_rec = *msg;
                                                                    sensor_rec = true;
                                                                    ROS_INFO("Received BridgeSensor message");
                                                                });
    auto chassis_pub = nh.advertise<cg_interfaces::ChassisStruct>("chassis_control", 1);
    ROS_INFO("Send BridgeControl Test");
    ROS_INFO("STEP1-1:Send a BridgeControl message to the virtual_bridge node in master mode");

    ros::spinOnce();
    ros::Duration(2).sleep();
    ROS_INFO("STEP0:publish chassis struct");
    cg_interfaces::ChassisStruct chassis_msg;
    chassis_msg.master = 1;
    chassis_msg.count = 2;
    chassis_msg.struct_ids = {1, 2, 0, 0};
    chassis_pub.publish(chassis_msg);
    ros::spinOnce();
    ros::Duration(2).sleep();

    cg_interfaces::BridgeControl control_msg;
    control_msg.id = 1;
    control_msg.torque_left = -100;
    control_msg.torque_right = 100;
    control_msg.brake_force = 0;
    control_msg.steer_angle = 1234;
    control_pub.publish(control_msg);
    ROS_INFO("STEP1-2:Check if the message is received by the virtual_bridge node in slave mode");
    while ((!control_rec) && ros::ok())
    {
        ros::spinOnce();
        ROS_INFO("Waiting for message");
        ros::Duration(0.1).sleep();
    }
    if (control_msg_rec.id == control_msg.id && control_msg_rec.torque_left == control_msg.torque_left &&
        control_msg_rec.torque_right == control_msg.torque_right &&
        control_msg_rec.brake_force == control_msg.brake_force &&
        control_msg_rec.steer_angle == control_msg.steer_angle)
        ROS_INFO("RESULT-1:PASS");
    else
        ROS_ERROR("RESULT-1:FAILED");

    ROS_INFO("STEP2-1:Send a BridgeSensor message to the virtual_bridge node in slave mode");
    cg_interfaces::BridgeSensor sensor_msg;
    sensor_msg.id = 1;
    sensor_msg.rpm_left = 1000;
    sensor_msg.rpm_right = -1000;
    sensor_pub.publish(sensor_msg);
    ROS_INFO("STEP2-2:Check if the message is received by the virtual_bridge node in master mode");
    while ((!sensor_rec) && ros::ok())
    {
        ros::spinOnce();
        ROS_INFO("Waiting for message");
        ros::Duration(0.1).sleep();
    }
    if (sensor_msg_rec.id == sensor_msg.id && sensor_msg_rec.rpm_left == sensor_msg.rpm_left &&
        sensor_msg_rec.rpm_right == sensor_msg.rpm_right)
        ROS_INFO("RESULT-2:PASS");
    else
        ROS_ERROR("RESULT-2:FAILED");
    ROS_INFO("Local loopback test node finished");
    ros::shutdown();
    return 0;
}